#include <ros/ros.h> 
#include <sensor_msgs/Imu.h>
#include <std_msgs/String.h>
#include <boost/asio.hpp>         // 包含boost库函数
#include <boost/bind.hpp>
#include <sstream>
using namespace boost::asio;  


int main(int argc, char** argv)
{
    // Step 2: Initialization:

    ros::init(argc, argv, "imu");     
    ros::NodeHandle n;  
    ros::Publisher IMU_pub = n.advertise<sensor_msgs::Imu>("IMU_data", 20);  

    io_service io;
    serial_port sp(io, "/dev/ttyUSB1");
    sp.set_option(serial_port::baud_rate(9600));                                  
    sp.set_option(  serial_port::flow_control( serial_port::flow_control::none ) ); 
    sp.set_option(  serial_port::parity( serial_port::parity::none ) );             
    sp.set_option( serial_port::stop_bits(  serial_port::stop_bits::one ) );        
    sp.set_option(  serial_port::character_size( 8 ) );                             
    ros::Rate loop_rate(1);
    while(ros::ok())
    {
        // Step 3 and Step 4 are optional, only if you want to use the request-response communication pattern
        // Step 3: Request quaternion Data from TransdcuerM 
        //uint16 toId = 309; // Node ID
        char*  txData;
        int    txSize;
        char rxData[1000];  
        boost::system::error_code err;  
        //sp.read_some(buffer(rxData, 50),err);  // Read from serial port buffer

            sp.read_some(buffer(rxData,50),err);  // Read from serial port buffer
            if (err)
            {
                ROS_INFO("Serial port read_some Error!");
               // printf("%s",err);
                //return -1;
            }
            else
            {
                ROS_INFO("Serial port data");
                //printf("data:");
                for(int i=0;i<33;i++)
                {
                    printf("%02X\n",rxData[i]);
                }
            }


        io.run();  
        //printf("buf"); 
        //printf("buf[0] = %c\n",rxData[0]); 
        ros::spinOnce();  
        loop_rate.sleep();  
    }
   // return 0;
}
